clc
clear all;
%  theta    kinematic: joint angle
%  d        kinematic: link offset
%  a        kinematic: link length
%  alpha    kinematic: link twist
%  sigma    kinematic: 0 if revolute, 1 if prismatic


L1=Link([0      -16.7           0      -pi/2       0     ]);
L2=Link([0       0           21.7      0          0     ]);
L3=Link([0       18.2           1.3       pi/2          0     ]);
L4=Link([0       -14.9           1.3       -pi/2          0     ]);
L5=Link([0       -18.2           3.0       -pi/2          0     ]);
L6=Link([0       42.7           0       0          0     ]);
r=SerialLink([L1,L2,L3,L4,L5,L6],'name','jiqiren');                                    %建立机器人模型
r.display();
q=[0 0 0 0 -pi/2  -pi/2 ]
% q=q'
% theta=q;                                                   %机器人初始各关节的角度
r.plot(q,'delay',1,'workspace', [-100,100,-80,80,-150,20],'scale',0.4);
teach(r);                                                                              %机器人示教

hold on;
N=10000;                                                           %随机次数
theta1=-90/180*pi+(90/180*pi+90/180*pi)*rand(N,1); %关节1限制
theta2=-30/180*pi+(43/180*pi+30/180*pi)*rand(N,1);   %关节2限制
% theta3=-200/180*pi+(70/180*pi+200/180*pi)*rand(N,1);  %关节3限制
theta4=0/180*pi+(90/180*pi+0/180*pi)*rand(N,1); %关节4限制
theta5=-135/180*pi+(15/180*pi+45/180*pi)*rand(N,1); %关节5限制
theta6=-90/180*pi+(90/180*pi+90/180*pi)*rand(N,1); %关节6限制
for n=1:1:N
    modmyt04=r.fkine([theta1(n),theta2(n),-theta2(n),theta4(n),theta5(n),theta6(n)]);
    T=transl(modmyt04);
    plot3(T(1,1),T(2,1),T(3,1),'b.','MarkerSize',2);
end